/*=========================================================================
	UberSim Source Code Release
	-------------------------------------------------------------------------
	Copyright (C) 2002 Manuela Veloso, Brett Browning, Mike Bowling,
	James Bruce; {mmv, brettb, mhb, jbruce}@cs.cmu.edu
	Erick Tryzelaar {erickt}@andrew.cmu.edu
	School of Computer Science, Carnegie Mellon University
	-------------------------------------------------------------------------
	This software is distributed under the GNU General Public License,
	version 2.  If you do not have a copy of this licence, visit
	www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
	Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
	in the hope that it will be useful, but WITHOUT ANY WARRANTY,
	including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
	-------------------------------------------------------------------------*/

#include <ode/ode.h>

#include "mmgr.h"

#include "Collision.h"
#include "CollisionManager.h"
#include "Node.h"
#include "Geometry.h"
#include "RigidBody.h"
#include "RigidBodyCollider.h"
#include "Simulator.h"
#include "Spatial.h"
#include "Surface.h"

/*******************************/

RigidBodyCollider::RigidBodyCollider (RigidBody* rigidBody)
{
	this->rigidBody = rigidBody;
	this->surface   = 0;

	if (rigidBody)
	{
		rigidBody->setRigidBodyCollider (this);
	}
}

/*******************************/

RigidBodyCollider::~RigidBodyCollider ()
{
	delete surface;
}

/*******************************/

RigidBody* RigidBodyCollider::getRigidBody ()
{
	return rigidBody;
}

/*******************************/

Surface* RigidBodyCollider::getSurface ()
{
	return surface;
}

/*******************************/

void RigidBodyCollider::setSurface (Surface* surface)
{
	this->surface = surface;
}

/*******************************/

void RigidBodyCollider::handleCollision (Geometry* geometry, Collision& collision)
{
	/*
		 dVector3 pos;
		 pos[0] = collision.position.x;
		 pos[1] = collision.position.y;
		 pos[2] = collision.position.z;

		 dVector3 normal;
		 normal[0] = collision.normal.x;
		 normal[1] = collision.normal.y;
		 normal[2] = collision.normal.z;

		 dContact contact;
		 contact.geom.pos    = pos;
		 contact.geom.normal = normal;
		 contact.geom.depth  = collision.depth;
		 contact.surface     = rigidBody->getSurface ()->getSurfaceParameters ();
		 */

	Simulator* simulator = Simulator::getInstance ();

	RigidBodyCollider* collider = dynamic_cast<RigidBodyCollider*> (geometry->getCollisionController ());

	if (collider)
	{
		RigidBody* otherRigidBody = collider->getRigidBody ();

		if (!simulator->getIsCollisionHandled (rigidBody, otherRigidBody))
		{
			Surface s;

			if (surface)
			{
				s = *surface;
			}

			Surface* otherSurface = collider->getSurface ();

			if (otherSurface)
			{
				s += *otherSurface;
			}

			collision.contact->surface = s.getSurfaceParameters ();

			simulator->addCollision (rigidBody, otherRigidBody, collision);
		}
	}
	else
	{
		Surface s;

		if (surface)
		{
			s = *surface;
		}

		collision.contact->surface = s.getSurfaceParameters ();

		Vector3 v = s.getFrictionDirection ();

		collision.contact->fdir1[0] = v.x;
		collision.contact->fdir1[1] = v.y;
		collision.contact->fdir1[2] = v.z;

		/*
		Spatial* parent = rigidBody->getModel ()->getParent ();
		if (parent)
		{
			// FIXME: Does not work for omni
			Vector3 v = (parent->getWorldRotation () * Vector3::UNIT_X);
			v.z       = 0;
			v.normalize ();

			dVector3 fdir1;
			fdir1[0] = v.x;
			fdir1[1] = v.y;
			fdir1[2] = v.z;

			collision.contact->fdir1 = fdir1;	
		}
		*/

		simulator->addCollision (rigidBody, collision);
	}
}

/*******************************/
